﻿using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
namespace RayDen.Library.Core.Primitives
{
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public class AABB
    {
        private Point min, max;

        public AABB()
        {
            this.min = new Point(1e14f);
            this.max = new Point(-1e14f);

        }


        public AABB(Point p)
        {
            this.min = new Point(p.x, p.y, p.z);
            this.max = new Point(p.x, p.y, p.z);
        }

        public AABB(ref Point p)
        {
            this.min = new Point(p.x, p.y, p.z);
            this.max = new Point(p.x, p.y, p.z);
        }

        public AABB(Point p1, Point p2)
        {
            this.min = new Point(
                                Math.Min(p1.x, p2.x),
                                Math.Min(p1.y, p2.y),
                                Math.Min(p1.z, p2.z));
            this.max = new Point(
                                Math.Max(p1.x, p2.x),
                                Math.Max(p1.y, p2.y),
                                Math.Max(p1.z, p2.z));
        }
        public AABB(ref Point p1, ref Point p2)
        {
            this.min = new Point(
                                Math.Min(p1.x, p2.x),
                                Math.Min(p1.y, p2.y),
                                Math.Min(p1.z, p2.z));
            this.max = new Point(
                                Math.Max(p1.x, p2.x),
                                Math.Max(p1.y, p2.y),
                                Math.Max(p1.z, p2.z));
        }
        public AABB(AABB cc)
        {
            this.min = cc.min;
            this.max = cc.max;

        }

        public AABB(IEnumerable<Point> point)
        {
            this.min = new Point(1e14f);
            this.max = new Point(-1e14f);
            foreach (var p in point)
            {
                this.min = new Point(
                                Math.Min(p.x, min.x),
                                Math.Min(p.y, min.y),
                                Math.Min(p.z, min.z));
                this.max = new Point(
                                    Math.Max(p.x, max.x),
                                    Math.Max(p.y, max.y),
                                    Math.Max(p.z, max.z));
            }
        }

        public static AABB Zero()
        {
            return new AABB(new Point(0f), new Point(0f));
        }

        public static AABB operator |(AABB b1, AABB b2)
        {
            AABB b = new AABB();

            b.min.x = (b1.Min.x < b2.Min.x ? b1.Min.x : b2.Min.x);
            b.max.x = (b1.max.x > b2.max.x ? b1.max.x : b2.max.x);

            b.min.y = (b1.Min.y < b2.Min.y ? b1.Min.y : b2.Min.y);
            b.max.y = (b1.max.y > b2.max.y ? b1.max.y : b2.max.y);

            b.min.z = (b1.Min.z < b2.Min.z ? b1.Min.z : b2.Min.z);
            b.max.z = (b1.max.z > b2.max.z ? b1.max.z : b2.max.z);

            return b;
        }
        public void fit(Vector p)
        {
            if (p.x < min.x)
                min.x = p.x;
            if (p.y < min.y)
                min.y = p.y;
            if (p.z < min.z)
                min.z = p.z;
            max.x = Math.Max(p.x, max.x);
            max.y = Math.Max(p.y, max.y);
            max.z = Math.Max(p.z, max.z);
        }
        public void fit(Point p)
        {
            if (p.x < min.x)
                min.x = p.x;
            if (p.y < min.y)
                min.y = p.y;
            if (p.z < min.z)
                min.z = p.z;
            max.x = Math.Max(p.x, max.x);
            max.y = Math.Max(p.y, max.y);
            max.z = Math.Max(p.z, max.z);
        }
        public Vector Size
        {
            get
            {
                return (max - min).Abs();
            }
        }


        public Vector Sum
        {
            get
            {
                return (Vector)(this.Min + this.Max);
            }
        }


        public Point Center
        {
            get
            {
                return 0.5f * this.min + 0.5f * this.max;
            }
        }

        public Point Min
        {
            get
            {
                return this.min;
            }
            set
            {
                this.min = value; ;
            }
        }

        public Point Max
        {
            get
            {
                return this.max;
            }
            set
            {
                this.max = value; ;
            }
        }


        public bool Overlaps(AABB b)
        {
            bool x = (this.max.x >= b.min.x) && (this.min.x <= b.max.x);
            bool y = (this.max.y >= b.min.y) && (this.min.y <= b.max.y);
            bool z = (this.max.z >= b.min.z) && (this.min.z <= b.max.z);
            return (x && y && z);
        }
        public bool Inside(Point pt)
        {
            return (pt.x >= this.min.x && pt.x <= this.max.x &&
                    pt.y >= this.min.y && pt.y <= this.max.y &&
                    pt.z >= this.min.z && pt.z <= this.max.z);
        }
        public void Expand(float delta)
        {
            this.min -= delta;
            this.max += delta;
        }
        public AABB ExpandThis(float delta)
        {
            this.min -= delta;
            this.max += delta;
            return this;
        }
        public float Volume()
        {
            Vector d = this.max - this.min;
            return d.x * d.y * d.z;
        }
        public float SurfaceArea()
        {
            Vector d = this.max - this.min;
            return 2f * (d.x * d.y + d.y * d.z + d.z * d.x);
        }
        public int MaximumExtent()
        {
            Vector diag = this.max - this.min;
            if (diag.x > diag.y && diag.x > diag.z)
                return 0;
            else if (diag.y > diag.z && diag.y > diag.x)
                return 1;
            else
                return 2;
        }

        public static AABB Union(AABB ths, Vector p)
        {
            var ret = new AABB(ths.min, ths.max)
            {
                min =
                {
                    x = Math.Min(ths.min.x, p.x),
                    y = Math.Min(ths.min.y, p.y),
                    z = Math.Min(ths.min.z, p.z)
                },
                max =
                {
                    x = Math.Max(ths.max.x, p.x),
                    y = Math.Max(ths.max.y, p.y),
                    z = Math.Max(ths.max.z, p.z)
                }
            };
            return ret;
        }

        public AABB Union(Point p)
        {
            var ret = this;
            ret.min.x = Math.Min(min.x, p.x);
            ret.min.y = Math.Min(min.y, p.y);
            ret.min.z = Math.Min(min.z, p.z);
            ret.max.x = Math.Max(max.x, p.x);
            ret.max.y = Math.Max(max.y, p.y);
            ret.max.z = Math.Max(max.z, p.z);
            return ret;
        }

        public AABB Union(Vector p)
        {
            var ret = this;
            ret.min.x = Math.Min(min.x, p.x);
            ret.min.y = Math.Min(min.y, p.y);
            ret.min.z = Math.Min(min.z, p.z);
            ret.max.x = Math.Max(max.x, p.x);
            ret.max.y = Math.Max(max.y, p.y);
            ret.max.z = Math.Max(max.z, p.z);
            return ret;
        }
        public AABB Union(AABB p)
        {
            var ret = new AABB
                {
                    min =
                        {
                            x = Math.Min(this.min.x, p.min.x),
                            y = Math.Min(this.min.y, p.min.y),
                            z = Math.Min(this.min.z, p.min.z)
                        },
                    max =
                        {
                            x = Math.Max(this.max.x, p.max.x),
                            y = Math.Max(this.max.y, p.max.y),
                            z = Math.Max(this.max.z, p.max.z)
                        }
                };
            return ret;
        }

        public static AABB Union(AABB ths, AABB p)
        {
            var ret = new AABB(ths.min, ths.max)
            {
                min =
                {
                    x = Math.Min(ths.min.x, p.min.x),
                    y = Math.Min(ths.min.y, p.min.y),
                    z = Math.Min(ths.min.z, p.min.z)
                },
                max =
                {
                    x = Math.Max(ths.max.x, p.max.x),
                    y = Math.Max(ths.max.y, p.max.y),
                    z = Math.Max(ths.max.z, p.max.z)
                }
            };
            return ret;
        }


        public static void FindMinMax<T>(T x0, T x1, T x2, out T min, out T max)
            where T : IComparable<T>
        {
            min = max = x0;
            if (x1.CompareTo(min) < 0)
                min = x1;
            if (x1.CompareTo(max) > 0)
                max = x1;
            if (x2.CompareTo(min) < 0)
                min = x2;
            if (x2.CompareTo(max) > 0)
                max = x2;
        }



        public bool IntersectTri(ref Point a_V0, ref Point a_V1, ref Point a_V2)
        {
            return IntersectTriBox(this.Center, this.Size, ref a_V0, ref a_V1, ref a_V2);
        }

        public static bool IntersectTriBox(Point a_BoxCentre, Vector a_BoxHalfsize, ref Point a_V0, ref Point a_V1, ref Point a_V2)
        {
            Vector v0, v1, v2, normal, e0, e1, e2;
            float min = 0, max = 0, p0, p1, p2, rad, fex, fey, fez;
            v0 = a_V0 - a_BoxCentre;
            v1 = a_V1 - a_BoxCentre;
            v2 = a_V2 - a_BoxCentre;
            e0 = v1 - v0;
            e1 = v2 - v1; e2 = v0 - v2;
            fex = Math.Abs(e0[0]);
            fey = Math.Abs(e0[1]);
            fez = Math.Abs(e0[2]);

            //AXISTEST_X01( e0[2], e0[1], fez, fey );( a, b, fa, fb )
            p0 = e0[2] * v0[1] - e0[1] * v0[2]; p2 = e0[2] * v2[1] - e0[1] * v2[2];
            if (p0 < p2) { min = p0; max = p2; } else { min = p2; max = p0; }
            rad = fez * a_BoxHalfsize[1] + fey * a_BoxHalfsize[2];
            if (min > rad || max < -rad) return false;

            //AXISTEST_Y02( e0[2], e0[0], fez, fex ); ( a, b, fa, fb )
            p0 = -e0[2] * v0[0] + e0[0] * v0[2]; p2 = -e0[2] * v2[0] + e0[0] * v2[2];
            if (p0 < p2) { min = p0; max = p2; } else { min = p2; max = p0; }
            rad = fez * a_BoxHalfsize[0] + fex * a_BoxHalfsize[2];
            if (min > rad || max < -rad) return false;

            //AXISTEST_Z12( e0[1], e0[0], fey, fex ); ( a, b, fa, fb )
            p1 = e0[1] * v1[0] - e0[0] * v1[1]; p2 = e0[1] * v2[0] - e0[0] * v2[1];
            if (p2 < p1) { min = p2; max = p1; } else { min = p1; max = p2; }
            rad = fey * a_BoxHalfsize[0] + fex * a_BoxHalfsize[1];
            if (min > rad || max < -rad) return false;

            fex = Math.Abs(e1[0]);
            fey = Math.Abs(e1[1]);
            fez = Math.Abs(e1[2]);

            //AXISTEST_X01( e1[2], e1[1], fez, fey );( a, b, fa, fb )
            p0 = e1[2] * v0[1] - e1[1] * v0[2]; p2 = e1[2] * v2[1] - e1[1] * v2[2];
            if (p0 < p2) { min = p0; max = p2; } else { min = p2; max = p0; }
            rad = fez * a_BoxHalfsize[1] + fey * a_BoxHalfsize[2];
            if (min > rad || max < -rad) return false;

            //AXISTEST_Y02( e1[2], e1[0], fez, fex );
            p0 = -e1[2] * v0[0] + e1[0] * v0[2]; p2 = -e1[2] * v2[0] + e1[0] * v2[2];
            if (p0 < p2) { min = p0; max = p2; } else { min = p2; max = p0; }
            rad = fez * a_BoxHalfsize[0] + fex * a_BoxHalfsize[2];
            if (min > rad || max < -rad) return false;

            //AXISTEST_Z0 ( e1[1], e1[0], fey, fex );
            p0 = e1[1] * v0[0] - e1[0] * v0[1]; p1 = e1[1] * v1[0] - e1[0] * v1[1];
            if (p0 < p1) { min = p0; max = p1; } else { min = p1; max = p0; }
            rad = fey * a_BoxHalfsize[0] + fex * a_BoxHalfsize[1];
            if (min > rad || max < -rad) return false;

            Vector fe = new Vector(ref e2).Abs();

            fex = fe.x;
            fey = fe.y;
            fez = fe.z;

            //AXISTEST_X2 ( e2[2], e2[1], fez, fey );
            p0 = e2[2] * v0[1] - e2[1] * v0[2]; p1 = e2[2] * v1[1] - e2[1] * v1[2];
            if (p0 < p1) { min = p0; max = p1; } else { min = p1; max = p0; }
            rad = fez * a_BoxHalfsize[1] + fey * a_BoxHalfsize[2];
            if (min > rad || max < -rad) return false;


            //AXISTEST_Y1 ( e2[2], e2[0], fez, fex );
            p0 = -e2[2] * v0[0] + e2[0] * v0[2]; p1 = -e2[2] * v1[0] + e2[0] * v1[2];
            if (p0 < p1) { min = p0; max = p1; } else { min = p1; max = p0; }
            rad = fez * a_BoxHalfsize[0] + fex * a_BoxHalfsize[2];
            if (min > rad || max < -rad) return false;

            //AXISTEST_Z12( e2[1], e2[0], fey, fex );
            p1 = e2[1] * v1[0] - e2[0] * v1[1]; p2 = e2[1] * v2[0] - e2[0] * v2[1];
            if (p2 < p1) { min = p2; max = p1; } else { min = p1; max = p2; }
            rad = fey * a_BoxHalfsize[0] + fex * a_BoxHalfsize[1];
            if (min > rad || max < -rad) return false;


            FindMinMax<float>(v0[0], v1[0], v2[0], out min, out max);
            if (min > a_BoxHalfsize[0] || max < -a_BoxHalfsize[0]) return false;
            FindMinMax<float>(v0[1], v1[1], v2[1], out min, out max);
            if (min > a_BoxHalfsize[1] || max < -a_BoxHalfsize[1]) return false;
            FindMinMax<float>(v0[2], v1[2], v2[2], out min, out max);
            if (min > a_BoxHalfsize[2] || max < -a_BoxHalfsize[2]) return false;
            normal = e0 ^ e1;
            if (!PlaneBoxOverlap(normal, v0, a_BoxHalfsize)) return false;
            return true;

        }

        public static bool PlaneBoxOverlap(Vector normal, Vector v0, Vector a_BoxHalfsize)
        {
            Vector vmin = new Vector(0f), vmax = new Vector(0f);
            for (int q = 0; q < 3; q++)
            {
                float v = v0[q];
                if (normal[q] > 0.0f)
                {
                    vmin[q] = -a_BoxHalfsize[q] - v;
                    vmax[q] = a_BoxHalfsize[q] - v;
                }
                else
                {
                    vmin[q] = a_BoxHalfsize[q] - v;
                    vmax[q] = -a_BoxHalfsize[q] - v;
                }
            }
            if ((normal & vmin) > 0.0f) return false;
            if ((normal & vmax) >= 0.0f) return true;
            return false;

        }

        public void BoundingSphere(out Point c, out float rad)
        {
            c = .5f * Min + .5f * Max;
            rad = Inside(c) ? (c - Max).Length : 0f;
        }

        public bool Intersect(IRay ray, out float hitt0, out float hitt1)
        {
            float t0 = ray.Min;
            float t1 = ray.Max;
            hitt0 = t0;
            hitt1 = t1;


            float invDirX = 1f / ray.Direction.x;
            if (!process_box_coord(ref t0, ref t1, min.x - ray.Origin.x, max.x - ray.Origin.x, invDirX))
                return false;

            float invDirY = 1f / ray.Direction.y;
            if (!process_box_coord(ref t0, ref t1, min.y - ray.Origin.y, max.y - ray.Origin.y, invDirY))
                return false;

            float invDirZ = 1f / ray.Direction.z;
            if (!process_box_coord(ref t0, ref t1, min.z - ray.Origin.z, max.z - ray.Origin.z, invDirZ))
                return false;

            /*
            for (int i = 0; i < 3; ++i) {
                // Update interval for _i_th bounding box slab
                float invRayDir = 1f / ray.Direction[i];
                if (!process_box_coord(ref t0, ref t1, min[i] - ray.Origin[i], max[i] - ray.Origin[i], invRayDir)) return false;
            }
             * */

            hitt0 = t0;
            hitt1 = t1;
            return true;
        }

        private static bool process_box_coord(ref float t0, ref float t1, float mnRO, float mxRO, float invRayDir)
        {
            float tNear = (mnRO) * invRayDir;
            float tFar = (mxRO) * invRayDir;
            // Update parametric interval from slab intersection $t$s
            if (tNear > tFar) swap(ref tNear, ref tFar);
            t0 = tNear > t0 ? tNear : t0;
            t1 = tFar < t1 ? tFar : t1;
            if (t0 > t1) return false;
            return true;
        }

        private static void swap(ref float tNear, ref  float tFar)
        {
            float tmp = tNear;
            tNear = tFar;
            tFar = tmp;
        }
        private static bool SmallEnough(float value)
        {
            return value > 0 && value < MathLab.Epsilon;
        }

        public bool IntersectC(ref IRay r, out float dist0, out float dist1)
        {
            var InvDir = 1f / r.Direction;
            dist0 = dist1 = 0;
            float q0 = float.MinValue;
            float q1 = float.MaxValue;
            if (SmallEnough(r.Direction.x))
            {
                if (r.Origin.x < min.x || r.Origin.x > max.x)
                    return false;
            }
            else
            {
                float r0 = (min.x - r.Origin.x) * InvDir.x;
                float r1 = (max.x - r.Origin.x) * InvDir.x;
                if (r0 > r1)
                {
                    float temp = r0;
                    r0 = r1;
                    r1 = temp;
                }
                if (r0 > q0)
                    q0 = r0;
                if (r1 < q1)
                    q1 = r1;
                if (q0 > q1)
                    return false;
                if (q1 < 0.0f)
                    return false;
            }
            if (SmallEnough(r.Direction.y))
            {
                if (r.Origin.y < min.y || r.Origin.y > max.y)
                    return false;
            }
            else
            {
                float r0 = (min.y - r.Origin.y) * InvDir.y;
                float r1 = (max.y - r.Origin.y) * InvDir.y;
                if (r0 > r1)
                {
                    float temp = r0;
                    r0 = r1;
                    r1 = temp;
                }
                if (r0 > q0)
                    q0 = r0;
                if (r1 < q1)
                    q1 = r1;
                if (q0 > q1)
                    return false;
                if (q1 < 0.0f)
                    return false;
            }
            if (SmallEnough(r.Direction.z))
            {
                if (r.Origin.z < min.z || r.Origin.z > max.z)
                    return false;
            }
            else
            {
                float r0 = (min.z - r.Origin.z) * InvDir.z;
                float r1 = (max.z - r.Origin.z) * InvDir.z;
                if (r0 > r1)
                {
                    float temp = r0;
                    r0 = r1;
                    r1 = temp;
                }
                if (r0 > q0)
                    q0 = r0;
                if (r1 < q1)
                    q1 = r1;
                if (q0 > q1)
                    return false;
                if (q1 < 0.0f)
                    return false;
            }
            dist0 = q0;
            dist1 = q1;
            if ((dist0 <= 0.0f) && (dist1 <= 0.0f))
                return false;
            //r.SetSegment(dist0, dist1);
            return true;
        }

        public void Reset()
        {
            this.min = new Point(1e14f);
            this.max = new Point(-1e14f);
        }


        public bool Inside(AABB box)
        {
            for (int idx = 0; idx < 3; idx++)
            {
                if (min[idx] > box.min[idx])
                    return false;
                if (max[idx] < box.max[idx])
                    return false;
            }
            return true;
        }
    }
}
